#include "packets.h"

void sendPacket(Packet* packet){
  Serial.write(packet->StartPacket);
  Serial.write(packet->MessageType);
  Serial.write((byte*)&(packet->DataLength),2);
  Serial.write((byte*)(packet->DataContent),packet->DataLength);
  Serial.write((byte*)&(packet->DataChecksum),2);
  Serial.write(packet->EndPacket);
}

void updateChecksum(Packet* packet){
  packet->DataChecksum=calculateChecksum(packet);
}

unsigned short calculateChecksum(Packet* packet){
  byte csumLow=0;
  byte csumHigh=0;
  unsigned short l;
  l=0;
  while(l<(packet->DataLength-1)){
    csumLow^=packet->DataContent[l++];
    csumHigh^=packet->DataContent[l++];
  }
  if(l<packet->DataLength)
    csumLow^=packet->DataContent[l++];
  return (csumHigh<<8|csumLow);
}

int readPacket(Packet* packet){
  int result=0;
  while((Serial.available()>6)&&(Serial.peek()!=PACKET_START))
    Serial.read();
  if(Serial.available()<=6)
    return 0;
  Serial.read();
  packet->MessageType=Serial.read();
  packet->DataLength=Serial.read()|(Serial.read()<<8);
  if(packet->DataLength>=(Serial.available()-3))
  {
    for(int i=0;i<packet->DataLength;i++)
      packet->DataContent[i]=Serial.read();
    packet->DataChecksum=Serial.read()|(Serial.read()<<8);
    packet->EndPacket=Serial.read();
    return 1;
  }
  return 0;
}

